#include <iostream>
#include "AStar.h"
#include "Node.h"

using namespace std;

//------------------------------------------------------------------------------
AStar::AStar(std::vector<int> vec, int columnCount, int rowCount , int initialColumn, int initialRow, int finalColumn, int finalRow)
: m_rowCount(rowCount),
  m_columnCount(columnCount),
  m_initialRow(initialRow),
  m_initialColumn(initialColumn),
  m_finalRow(finalRow),
  m_finalColumn(finalColumn)
{

	ANode* newNode = NULL;

	for (int i = 0; i < rowCount; i++) {
		for (int j = 0; j < columnCount; j++) {
			float cost = float(vec[i * columnCount + j]);
			newNode = new ANode();
			if (newNode) {
				newNode->setRow(i);
				newNode->setColumn(j);
				if(cost != 0)
				{
					cost = -1;
				}
				newNode->setCost(cost);
				m_nodes.push_back(newNode);
			}
		}


	}

	calculateAdjacents();
}
//------------------------------------------------------------------------------
AStar::~AStar() {

	for (unsigned int i = 0; i < m_nodes.size(); i++) {

		if (m_nodes[i]) {
			delete m_nodes[i];
			m_nodes[i] = NULL;
		}
	}
}
//------------------------------------------------------------------------------
std::vector<AStarResult*> AStar::execute() {

	if (m_nodes.empty()) {
		return std::vector<AStarResult*>();
	}

	// Push the first node to the opened list
	m_openedNodes.push_back(getNode(m_initialRow, m_initialColumn));

	while (!m_openedNodes.empty()) {
		ANode* node = getLeastCostlyNode();
		if (node) {
			// The solution was found
			if (node->getRow() == m_finalRow && node->getColumn() == m_finalColumn) {
				return calculateResult(node);
			}
			else {
				closeNode(node);
				openAdjacents(node);
			}
		}
	}

	return std::vector<AStarResult*>();
}
//------------------------------------------------------------------------------
std::vector<AStarResult*> AStar::getResult() {

	return m_result;
}
//------------------------------------------------------------------------------
ANode* AStar::getLeastCostlyNode() {

	if (m_openedNodes.empty()) {
		return NULL;
	}

	ANode* finalNode = getNode(m_finalRow, m_finalColumn);
	ANode* leastCostly = m_openedNodes[0];
	float min = getDistance(leastCostly, finalNode);
	
	if (finalNode) {
		for (unsigned int i = 1; i < m_openedNodes.size(); i++) {
			float cost = getDistance(m_openedNodes[i], finalNode);
			if (cost < min) {
				min = cost;
				leastCostly = m_openedNodes[i];
			}
		}
	}

	return leastCostly;
}
//------------------------------------------------------------------------------
void AStar::closeNode(ANode* node) {

	if (!isInOpened(node)) {
		return;
	}

	removeFromOpened(node);
	m_closedNodes.push_back(node);
}
//------------------------------------------------------------------------------
void AStar::openAdjacents(ANode* node) {

	std::vector<ANode*> adjacents = node->getAdjacents();
	for (unsigned int i = 0; i < adjacents.size(); i++) {
		if (!isInOpened(adjacents[i]) && !isInClosed(adjacents[i]) && adjacents[i]->getCost() != -1) {
			adjacents[i]->setParent(node);
			m_openedNodes.push_back(adjacents[i]);
		}
	}
}
//------------------------------------------------------------------------------
std::vector<AStarResult*> AStar::calculateResult(ANode* node) {

	ANode* currentNode = node;
	while (currentNode != NULL) {
		AStarResult* result = new AStarResult(currentNode->getRow(), currentNode->getColumn());
		m_result.push_back(result);
		currentNode = currentNode->getParent();
	}
	

	return m_result;
}
//------------------------------------------------------------------------------
bool AStar::isInOpened(ANode* node) const {

	for (unsigned int i = 0; i < m_openedNodes.size(); i++) {
		if (m_openedNodes[i] == node) {
			return true;
		}
	}

	return false;
}
//------------------------------------------------------------------------------
bool AStar::isInClosed(ANode* node) const {

	for (unsigned int i = 0; i < m_closedNodes.size(); i++) {
		if (m_closedNodes[i] == node) {
			return true;
		}
	}

	return false;
}
//------------------------------------------------------------------------------
void AStar::removeFromOpened(ANode* node) {

	std::vector<ANode*>::iterator it = m_openedNodes.begin();
	while (it != m_openedNodes.end()) {
		if (*it == node) {
			m_openedNodes.erase(it);
			break;
		}

		it++;
	}
}
//------------------------------------------------------------------------------
void AStar::calculateAdjacents() {

	int currentRow = 0;
	int currentColumn = 0;

	// Calculate the adjacents of each node
	for (unsigned int i = 0; i < m_nodes.size(); i++) {
		currentRow = m_nodes[i]->getRow();
		currentColumn = m_nodes[i]->getColumn();

		if (currentColumn - 1 >= 0) {
			ANode* node = getNode(currentRow, currentColumn - 1);
			if (node) {
				m_nodes[i]->addAdjacent(node);
			}
		}

		if (currentColumn + 1 < m_columnCount) {
			ANode* node = getNode(currentRow, currentColumn + 1);
			if (node) {
				m_nodes[i]->addAdjacent(node);
			}
		}

		if (currentRow - 1 >= 0) {
			ANode* node = getNode(currentRow - 1, currentColumn);
			if (node) {
				m_nodes[i]->addAdjacent(node);
			}
		}

		if (currentRow + 1 < m_rowCount) {
			ANode* node = getNode(currentRow + 1, currentColumn);
			if (node) {
				m_nodes[i]->addAdjacent(node);
			}
		}
	}
}
//------------------------------------------------------------------------------
ANode* AStar::getNode(int row, int column) {

	for (unsigned int i = 0; i < m_nodes.size(); i++) {
		if (row == m_nodes[i]->getRow() && column == m_nodes[i]->getColumn()) {
			return m_nodes[i];
		}
	}

	return NULL;
}
//------------------------------------------------------------------------------
float AStar::getDistance(ANode* initialNode, ANode* finalNode) {
	
	return initialNode->getCost() + (abs(initialNode->getRow() - finalNode->getRow()) + abs(initialNode->getColumn() - finalNode->getColumn()));
}
//------------------------------------------------------------------------------